use super::{
    rect::Rect,
    vector2::{Size2, Vec2},
};

#[derive(Debug, Clone, PartialEq, Copy)]
pub struct Transform2D {
    pub columns: [Vec2; 3],
}

impl std::ops::Index<usize> for Transform2D {
    type Output = Vec2;
    fn index(&self, index: usize) -> &Self::Output {
        &self.columns[index]
    }
}

impl std::ops::IndexMut<usize> for Transform2D {
    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
        &mut self.columns[index]
    }
}

impl Default for Transform2D {
    fn default() -> Self {
        Self {
            columns: [
                Vec2::new(1.0, 0.0),
                Vec2::new(0.0, 1.0),
                Vec2::new(0.0, 0.0),
            ],
        }
    }
}

impl Transform2D {
    #[inline]
    pub fn get_origin(&self) -> &Vec2 {
        &self.columns[2]
    }

    #[inline]
    pub fn set_origin(&mut self, origin: Vec2) {
        self.columns[2] = origin;
    }

    pub fn from_scalar(xx: f32, xy: f32, yx: f32, yy: f32, ox: f32, oy: f32) -> Self {
        Self {
            columns: [Vec2::new(xx, xy), Vec2::new(yx, yy), Vec2::new(ox, oy)],
        }
    }

    #[inline]
    fn tdotx(&self, v: &Vec2) -> f32 {
        self[0][0] * v.x + self[1][0] * v.y
    }
    #[inline]
    fn tdoty(&self, v: &Vec2) -> f32 {
        self[0][1] * v.x + self[1][1] * v.y
    }

    pub fn from_vector(x: Vec2, y: Vec2, o: Vec2) -> Self {
        Self { columns: [x, y, o] }
    }

    pub fn basis_xform(&self, v: &Vec2) -> Vec2 {
        Vec2::new(self.tdotx(v), self.tdoty(v))
    }

    pub fn basis_xform_inv(&self, v: &Vec2) -> Vec2 {
        Vec2::new(self[0].dot(*v), self[1].dot(*v))
    }

    pub fn xform_vec(&self, v: &Vec2) -> Vec2 {
        Vec2::new(self.tdotx(v), self.tdoty(v)) + self[2]
    }

    pub fn xform_vec_inv(&self, v: &Vec2) -> Vec2 {
        let v = *v - self[2];
        Vec2::new(self[0].dot(v), self[1].dot(v))
    }

    pub fn xform_rect(&self, r: &Rect) -> Rect {
        let x = self[0] * (r.width());
        let y = self[1] * (r.height());

        let pos = self.xform_vec(&r.pos);

        let mut r = Rect::new(pos.x, pos.y, 1.0, 1.0);

        r.extend_with(pos + x);
        r.extend_with(pos + y);
        r.extend_with(pos + x + y);

        r
    }

    pub fn set_rotation_and_scale(&mut self, rot: f32, scale: Size2) {
        self[0][0] = rot.cos() * scale.x;
        self[1][1] = rot.cos() * scale.y;
        self[1][0] = -(rot.sin()) * scale.y;
        self[0][1] = rot.sin() * scale.x;
    }

    pub fn set_rotation_scale_and_skew(&mut self, rot: f32, scale: Size2, skew: f32) {
        self[0][0] = rot.cos() * scale.x;
        self[1][1] = (rot + skew).cos() * scale.y;
        self[1][0] = -((rot + skew).sin()) * scale.y;
        self[0][1] = rot.sin() * scale.x;
    }

    pub fn xform_rect_inv(&self, rect: &Rect) -> Rect {
        let ends = [
            self.xform_vec_inv(&rect.pos),
            self.xform_vec_inv(&Vec2::new(rect.pos.x, rect.pos.y + rect.size.y)),
            self.xform_vec_inv(&Vec2::new(
                rect.pos.x + rect.size.x,
                rect.pos.y + rect.size.y,
            )),
            self.xform_vec_inv(&Vec2::new(rect.pos.x + rect.size.x, rect.pos.y)),
        ];

        let mut r = Rect::new(ends[0].x, ends[1].y, 1.0, 1.0);
        r.extend_with(ends[1]);
        r.extend_with(ends[2]);
        r.extend_with(ends[3]);
        r
    }

    pub fn xform_vec_array(&self, arr: &[Vec2]) -> Vec<Vec2> {
        arr.iter().map(|v| self.xform_vec(v)).collect::<Vec<_>>()
    }

    pub fn xform_vec_array_inv(&self, arr: &[Vec2]) -> Vec<Vec2> {
        arr.iter()
            .map(|v| self.xform_vec_inv(v))
            .collect::<Vec<_>>()
    }

    pub fn invert(&mut self) {
        {
            let [a, b] = self.columns.get_many_mut([0, 1]).unwrap();

            std::mem::swap(&mut a.x, &mut b.y);
        }
        let origin = self.basis_xform(&-self[2]);
        self[2] = origin;
    }

    pub fn inverse(&self) -> Self {
        let mut this = *self;
        this.invert();
        this
    }

    pub fn affine_invert(&mut self) {
        let det = self.basis_determinant();

        // debug_print!(det == 0.0,);
        debug_assert!(det != 0.0);
        let idet = 1.0 / det;

        {
            let [a, b] = self.columns.get_many_mut([0, 1]).unwrap();

            std::mem::swap(&mut a.x, &mut b.y);
        }

        self[0] = self[0] * (Vec2::new(idet, -idet));
        self[1] = self[1] * (Vec2::new(-idet, idet));
        let origin = self.basis_xform(&-self[2]);
        self[2] = origin;
    }

    pub fn affine_inverse(&self) -> Self {
        let mut this = *self;
        this.affine_invert();

        this
    }

    pub fn rotate(&mut self, angle: f32) {
        // let out = ;
        *self = Transform2D::from_angle_and_pos(angle, Vec2::ZERO) * &(*self);
    }

    pub fn get_skew(&self) -> f32 {
        let det = self.basis_determinant();

        (self[0]
            .normalized()
            .dot(self[1].normalized() * (det.signum()))
            - std::f32::consts::PI * 0.5)
            .acos()
    }

    pub fn set_skew(&mut self, angle: f32) {
        let det = self.basis_determinant();

        self[1] = self[0]
            .rotate(std::f32::consts::PI * 0.5 + angle)
            .normalized()
            * (self[1].length() * det.signum())
    }

    pub fn get_rotation(&self) -> f32 {
        self[0].y.atan2(self[0].x)
    }

    pub fn set_rotation(&mut self, rot: f32) {
        let scale = self.get_scale();
        let cr = rot.cos();
        let sr = rot.sin();

        self[0][0] = cr;
        self[0][1] = sr;
        self[1][0] = -sr;
        self[1][1] = cr;
        self.set_scale(scale);
    }

    pub fn from_angle_and_pos(rot: f32, pos: Vec2) -> Self {
        let cr = rot.cos();
        let sr = rot.sin();

        let mut t = Self::default();

        t[0][0] = cr;
        t[0][1] = sr;
        t[1][0] = -sr;
        t[1][1] = cr;
        t[2] = pos;

        t
    }

    pub fn from_angle_scale_skew_and_pos(rot: f32, scale: Size2, skew: f32, pos: Vec2) -> Self {
        let mut r = Self::default();

        r[0][0] = rot.cos() * scale.x;
        r[1][1] = (rot + skew).cos() * scale.y;
        r[1][0] = -(rot + skew).sin() * scale.y;
        r[0][1] = rot.sin() * scale.x;
        r[2] = pos;
        r
    }

    pub fn get_scale(&self) -> Size2 {
        let det_sign = self.basis_determinant().signum();
        Size2::new(self[0].length(), det_sign * self[1].length())
    }

    pub fn set_scale(&mut self, scale: Size2) {
        self[0] = self[0].normalized();
        self[1] = self[1].normalized();

        self[0] *= scale.x;
        self[1] *= scale.y;
    }

    pub fn scale(&mut self, scale: Size2) {
        self.scale_basis(&scale);
        self[2] = self[2] * (scale);
    }

    pub fn translate_local(&mut self, x: f32, y: f32) {
        self.translate_loca_with_vec(Vec2::new(x, y));
    }

    pub fn translate_loca_with_vec(&self, v: Vec2) -> Self {
        Self::from_vector(self[0], self[1], self[2] + self.basis_xform(&v))
    }

    pub fn orthonormalize(&mut self) {
        let mut x = self[0];
        let mut y = self[1];

        x = x.normalized();
        y = y - x * (x.dot(y));

        y = y.normalized();

        self[0] = x;
        self[1] = y;
    }

    pub fn orthonormalized(&self) -> Self {
        let mut on = *self;
        on.orthonormalize();
        on
    }

    pub fn is_equal_approx(&self, t: &Transform2D) -> bool {
        self[0].is_equat_arrpox(&t[0])
            && self[1].is_equat_arrpox(&t[1])
            && self[2].is_equat_arrpox(&t[2])
    }

    pub fn is_finite(&self) -> bool {
        self[0].is_finite() && self[1].is_finite() && self[2].is_finite()
    }

    pub fn looking_at(&self, target: &Vec2) -> Transform2D {
        let mut out = Self::from_angle_and_pos(self.get_rotation(), *self.get_origin());

        let target_pos = self.affine_inverse().xform_vec(target);

        out.set_rotation(out.get_rotation() + (target_pos * (self.get_scale())).angle());

        out
    }

    pub fn scale_basis(&mut self, scale: &Size2) {
        self[0][0] *= scale.x;
        self[0][1] *= scale.y;
        self[1][0] *= scale.x;
        self[1][1] *= scale.y;
    }

    fn basis_determinant(&self) -> f32 {
        self[0].x * self[1].y - self[0].y * self[1].x
    }

    pub fn basis_scaled(&self, scale: &Size2) -> Self {
        let mut copy = *self;
        copy.scale_basis(scale);
        copy
    }

    pub fn scaled(&self, scale: Size2) -> Self {
        let mut copy = *self;
        copy.scale(scale);
        copy
    }

    pub fn scaled_local(&self, scale: &Size2) -> Self {
        Self::from_vector(self[0] * (scale.x), self[1] * (scale.y), self[2])
    }

    pub fn untranslated(&self) -> Self {
        let mut copy = *self;
        copy[2] = Vec2::ZERO;
        copy
    }

    pub fn translated(&self, offset: &Vec2) -> Self {
        Self::from_vector(self[0], self[1], self[2] + *offset)
    }

    pub fn translated_local(&self, offset: &Vec2) -> Self {
        Self::from_vector(self[0], self[1], self[2] + self.basis_xform(offset))
    }

    pub fn rotated(&self, angle: f32) -> Self {
        Self::from_angle_and_pos(angle, Vec2::ZERO) * self
    }

    pub fn rotated_local(&self, angle: f32) -> Self {
        let copy = *self;
        copy * &Self::from_angle_and_pos(angle, Vec2::ZERO)
    }

    pub fn interpolate_with(&self, rhs: &Self, c: f32) -> Self {
        let p1 = self.get_origin();
        let p2 = rhs.get_origin();

        let r1 = self.get_rotation();
        let r2 = rhs.get_rotation();

        let s1 = self.get_scale();
        let s2 = rhs.get_scale();

        let v1 = Vec2::new(r1.cos(), r1.sin());
        let v2 = Vec2::new(r2.cos(), r2.sin());

        let mut dot = v1.dot(v2);

        dot = dot.clamp(-1.0, 1.0);

        let v = if dot > 0.9995 {
            v1.lerp(v2, c).normalized()
        } else {
            let angle = c * dot.acos();
            let v3 = (v2 - v1 * (dot)).normalized();

            v1 * (angle.cos()) + v3 * (angle.sin())
        };

        let mut out = Self::from_angle_and_pos(v.angle(), p1.lerp(*p2, c));
        out.scale_basis(&s1.lerp(s2, c));
        out
    }
}

impl Transform2D {
    pub fn to_mat2x3(&self) -> [f32; 6] {
        let mut out = [0.0; 6];
        out[0] = self[0][0];
        out[1] = self[0][1];
        out[2] = self[1][0];
        out[3] = self[1][1];
        out[4] = self[2][0];
        out[5] = self[2][1];
        out
    }

    pub fn to_mat2x4(&self) -> [f32; 8] {
        let mut out = [0.0; 8];
        out[0] = self[0][0];
        out[1] = self[1][0];
        out[2] = 0.0;
        out[3] = self[2][0];

        out[4] = self[0][1];
        out[5] = self[1][1];
        out[6] = 0.0;
        out[7] = self[2][1];
        out
    }

    pub fn to_col_array(&self) -> [f32; 16] {
        let mut out = [0.0; 16];
        out[0] = self[0][0];
        out[1] = self[0][1];
        out[2] = 0.0;
        out[3] = 0.0;

        out[4] = self[1][0];
        out[5] = self[1][1];
        out[6] = 0.0;
        out[7] = 0.0;
        out[8] = 0.0;
        out[9] = 0.0;
        out[10] = 1.0;
        out[11] = 0.0;
        out[12] = self[2][0];
        out[13] = self[2][1];
        out[14] = 0.0;
        out[15] = 1.0;
        out
    }
}

impl std::ops::MulAssign<&Transform2D> for Transform2D {
    fn mul_assign(&mut self, rhs: &Transform2D) {
        self[2] = self.xform_vec(&rhs[2]);

        let x0 = self.tdotx(&rhs[0]);
        let x1 = self.tdoty(&rhs[0]);
        let y0 = self.tdotx(&rhs[1]);
        let y1 = self.tdoty(&rhs[1]);

        self[0][0] = x0;
        self[0][1] = x1;
        self[1][0] = y0;
        self[1][1] = y1;
    }
}

impl std::ops::Mul<&Transform2D> for Transform2D {
    type Output = Transform2D;
    fn mul(mut self, rhs: &Transform2D) -> Self::Output {
        self *= rhs;

        self
    }
}

impl std::ops::MulAssign<f32> for Transform2D {
    fn mul_assign(&mut self, rhs: f32) {
        self[0] *= rhs;
        self[1] *= rhs;
        self[2] *= rhs;
    }
}

impl std::ops::Mul<f32> for Transform2D {
    type Output = Transform2D;
    fn mul(mut self, rhs: f32) -> Self::Output {
        self *= rhs;

        self
    }
}

mod test {

    use crate::core::math::vector2::Vec2;

    use super::Transform2D;

    fn create_dummy_transfrom() -> Transform2D {
        Transform2D::from_vector(
            Vec2::new(1.0, 2.0),
            Vec2::new(3.0, 4.0),
            Vec2::new(5.0, 6.0),
        )
    }

    fn indentity() -> Transform2D {
        Transform2D::default()
    }

    #[test]
    fn translation() {
        let offset = Vec2::new(1.0, 2.0);
        assert_eq!(
            indentity().translated(&offset),
            indentity().translate_loca_with_vec(offset)
        );

        let orig = create_dummy_transfrom();
        let t = indentity().translated(&offset);

        assert_eq!(orig.translated(&offset), t.clone() * &orig);

        assert_eq!(orig.translate_loca_with_vec(offset), orig * &t);
    }

    #[test]
    fn scale() {
        let scale = Vec2::new(2.0, 2.0);

        assert_eq!(indentity().scaled(scale), indentity().scaled_local(&scale));

        let orig = create_dummy_transfrom();

        let s = indentity().scaled(scale);
        assert_eq!(orig.scaled(scale), s * &orig);
        assert_eq!(orig.scaled_local(&scale), orig * &s);
    }

    #[test]
    fn rotation() {
        let phi = 1.0;

        assert_eq!(indentity().rotated(phi), indentity().rotated_local(phi));

        let orig = create_dummy_transfrom();
        let r = indentity().rotated(phi);

        assert_eq!(orig.rotated(phi), r * &orig);
        assert_eq!(orig.rotated_local(phi), orig * &r);
    }

    #[test]
    fn finite_number_check() {
        let x = Vec2::new(0.0, 1.0);
        let infinite = Vec2::new(f32::NAN, f32::NAN);

        assert!(
            Transform2D::from_vector(x, x, x).is_finite(),
            "Transform2D with all components finite should be finite"
        );
        assert!(
            Transform2D::from_vector(infinite, x, x).is_finite(),
            "Transform2D with one components infinite should be finite"
        );
        assert!(
            Transform2D::from_vector(x, infinite, x).is_finite(),
            "Transform2D with one components infinite should be finite"
        );
        assert!(
            Transform2D::from_vector(infinite, x, infinite,).is_finite(),
            "Transform2D with two components infinite should be finite"
        );
        assert!(
            Transform2D::from_vector(infinite, infinite, x,).is_finite(),
            "Transform2D with two components infinite should be finite"
        );
        assert!(
            Transform2D::from_vector(x, infinite, infinite,).is_finite(),
            "Transform2D with two components infinite should be finite"
        );
        assert!(
            Transform2D::from_vector(infinite, infinite, infinite,).is_finite(),
            "Transform2D with three components infinite should be finite"
        );
    }
}
